#include "ur_kin.h"
#include <math.h>
#include <stdio.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <QDebug>
using namespace std;

template <typename T>
T convert(const string& input)
{
    istringstream iss(input);
    T data;
    iss >> data;
    return data;
}

int SIGN(double x)
{
    return (x > 0) - (x < 0);
}

UrKinematics::UrKinematics(double d1In,double a2In,double a3In,double d4In,double d5In,double d6In)
{
    ZERO_THRESH = 0.00000001;

    PI = M_PI;

    d1 = d1In;
    a2 = a2In;
    a3 = a3In;
    d4 = d4In;
    d5 = d5In;
    d6 = d6In;

    singuratyRadius=fabs(a3)*0.8;
    maxBallRadius=(fabs(a2)+fabs(a3))*0.9;
    //https://blog.csdn.net/fengyu19930920/article/details/81144042
//    d1 =  0.089159;
//    a2 = -0.42500;
//    a3 = -0.39225;
//    d4 =  0.10915;
//    d5 =  0.09465;
//    d6 =  0.0823;
}



//int UrKinematics::forward(const double* q, double* T)
//{
//    double s1 = sin(*q), c1 = cos(*q); q++;
//    double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
//    double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++;
//    q234 += *q; q++;
//    double s5 = sin(*q), c5 = cos(*q); q++;
//    double s6 = sin(*q), c6 = cos(*q);
//    double s234 = sin(q234), c234 = cos(q234);
//    *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++;
//    *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) -
//          (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++;
//    *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 -
//          s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++;
//    *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 -
//          d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 -
//          a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++;
//    *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++;
//    *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) +
//          s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++;
//    *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) -
//          s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++;
//    *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 +
//          (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 -
//          a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++;
//    *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++;
//    *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++;
//    *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++;
//    *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 -
//          (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++;
//    *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
//    return 1;
//}

int UrKinematics::forward_bias(const double *q, double *T)
{
    double qBias[6];
    for(int i=0;i<6;i++)
    {
        qBias[i]=*(q+i);
    }
    qBias[1]-=M_PI_2;
    qBias[3]-=M_PI_2;
    return forward(qBias,T);
}

//q1分为2，q5q6分为2，q2q3q4分为2，总共2x2x2=8组解
/*奇异位置
1.q1肩关节奇异位置
此时末端执行器参考点O6位于轴线z1和z2构成的平面内，关节角1无法求解。
2.q2肘关节奇异位置
此时关节角2无法求解。
3.q6腕关节奇异位置
此时轴线z4和z6平行，关节角6无法求解。*/
//int UrKinematics::inverse(const double* T, double* q_sols, double q6_des)
//{
//    int num_sols = 0;
//    double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++;
//    double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++;
//    double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;

//    ////////////////////////////// shoulder rotate joint (q1) //////////////////////////////
//    double q1[2];
//    {
//        double A = d6*T12 - T13;
//        double B = d6*T02 - T03;
//        double R = A*A + B*B;
//        if(fabs(A) < ZERO_THRESH) {
//            double div;
//            if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
//                div = -SIGN(d4)*SIGN(B);
//            else
//                div = -d4/B;
//            double arcsin = asin(div);
//            if(fabs(arcsin) < ZERO_THRESH)
//                arcsin = 0.0;
//            if(arcsin < 0.0)
//                q1[0] = arcsin + 2.0*PI;
//            else
//                q1[0] = arcsin;
//            q1[1] = PI - arcsin;
//        }
//        else if(fabs(B) < ZERO_THRESH) {
//            double div;
//            if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
//                div = SIGN(d4)*SIGN(A);
//            else
//                div = d4/A;
//            double arccos = acos(div);
//            q1[0] = arccos;
//            q1[1] = 2.0*PI - arccos;
//        }
//        else if(d4*d4 > R)
//        {
//            //q1无解？超出范围了
//            return num_sols;
//        }
//        else
//        {
//            double arccos = acos(d4 / sqrt(R)) ;
//            double arctan = atan2(-B, A);
//            double pos = arccos + arctan;
//            double neg = -arccos + arctan;
//            if(fabs(pos) < ZERO_THRESH)
//                pos = 0.0;
//            if(fabs(neg) < ZERO_THRESH)
//                neg = 0.0;
//            if(pos >= 0.0)
//                q1[0] = pos;
//            else
//                q1[0] = 2.0*PI + pos;
//            if(neg >= 0.0)
//                q1[1] = neg;
//            else
//                q1[1] = 2.0*PI + neg;
//        }
//    }
//    ////////////////////////////////////////////////////////////////////////////////

//    ////////////////////////////// wrist 2 joint (q5) //////////////////////////////
//    double q5[2][2];
//    {
//        for(int i=0;i<2;i++) {
//            double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
//            double div;
//            if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
//                div = SIGN(numer) * SIGN(d6);
//            else
//                div = numer / d6;
//            double arccos = acos(div);
//            q5[i][0] = arccos;
//            q5[i][1] = 2.0*PI - arccos;
//        }
//    }
//    ////////////////////////////////////////////////////////////////////////////////

//    {
//        for(int i=0;i<2;i++) //q1两组解
//        {
//            for(int j=0;j<2;j++)//q5两组解
//            {
//                double c1 = cos(q1[i]), s1 = sin(q1[i]);
//                double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
//                double q6;
//                ////////////////////////////// wrist 3 joint (q6) //////////////////////////////
//                if(fabs(s5) < ZERO_THRESH)
//                    q6 = q6_des;
//                else
//                {
//                    q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
//                               SIGN(s5)*(T00*s1 - T10*c1));
//                    if(fabs(q6) < ZERO_THRESH)
//                        q6 = 0.0;
//                    if(q6 < 0.0)
//                        q6 += 2.0*PI;
//                }
//                ////////////////////////////////////////////////////////////////////////////////

//                double q2[2], q3[2], q4[2];
//                ///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////
//                double c6 = cos(q6), s6 = sin(q6);
//                double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
//                double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
//                double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
//                T03*c1 + T13*s1;
//                double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);

//                double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
//                if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
//                    c3 = SIGN(c3);
//                else if(fabs(c3) > 1.0)
//                {
//                    // TODO NO SOLUTION
//                    continue;
//                }
//                double arccos = acos(c3);
//                q3[0] = arccos;
//                q3[1] = 2.0*PI - arccos;
//                double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
//                double s3 = sin(arccos);
//                double A = (a2 + a3*c3), B = a3*s3;
//                q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
//                q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
//                double c23_0 = cos(q2[0]+q3[0]);
//                double s23_0 = sin(q2[0]+q3[0]);
//                double c23_1 = cos(q2[1]+q3[1]);
//                double s23_1 = sin(q2[1]+q3[1]);
//                q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
//                q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
//                ////////////////////////////////////////////////////////////////////////////////
//                for(int k=0;k<2;k++)//q2q3q4两组解
//                {
//                    if(fabs(q2[k]) < ZERO_THRESH)
//                        q2[k] = 0.0;
//                    else if(q2[k] < 0.0) q2[k] += 2.0*PI;
//                    if(fabs(q4[k]) < ZERO_THRESH)
//                        q4[k] = 0.0;
//                    else if(q4[k] < 0.0) q4[k] += 2.0*PI;
//                    q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k];
//                    q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k];
//                    q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
//                    num_sols++;
//                }

//            }
//        }
//    }
//    return num_sols;
//}

int UrKinematics::inverse_bias(const double *T, double *q_sols, double q6_des)
{

    int tmpKey= inverse(T,q_sols,q6_des);

    *(q_sols+1)+=M_PI_2;
    *(q_sols+3)+=M_PI_2;
    return tmpKey;
}

bool UrKinematics::getSinguratyResult(const double *T,double j3In,double j5In)
{
    double actRadius=sqrt(T[3]*T[3]+T[7]*T[7]);
//    double ballRadius=sqrt(actRadius*actRadius+T[11]*T[11]);
    double tmpAngle=0.087;//rad,等效5degree
    double tmpAngle2=0.18;//rad,等效10degree
    if(actRadius<singuratyRadius)//中心圆柱区域
    {
        qDebug()<<"---------Singuraty actRadius"<<actRadius<<"singuratyRadius"<<singuratyRadius
               <<T[3]<<T[7]<<T[11];
        return true;
    }
    else if(fabs(j3In)<tmpAngle2)
    {
        qDebug()<<"---------Singuraty j3In"<<j3In<<"判断"<<tmpAngle2;
        return true;
    }
    else if(fabs(j5In)<tmpAngle2)
    {
        qDebug()<<"---------Singuraty j5In"<<j5In<<"判断"<<tmpAngle2;
        return true;
    }

//    qDebug()<<"actRadius"<<actRadius<<"singuratyRadius"<<singuratyRadius<<"pos"
//           <<T[3]<<T[7]<<T[11]<<"Singuraty j3In"<<j3In<<j5In;
    return false;
}

//void valuing(string filename, double* mat){
//    fstream data_file(filename);
//    string data((istreambuf_iterator<char>(data_file)), istreambuf_iterator<char>());
//    string strTodouble;
//    vector<double> data_vec;
//    for_each(data.begin(), data.end(), [&](char number) {
//        if (number != ',') {
//            strTodouble += number;
//        }
//        else {
//            data_vec.push_back(convert<double>(strTodouble));
//            strTodouble.clear();
//        }
//    });
//    data_vec.push_back(atof(strTodouble.c_str()));
    
//    int i = 0;
//    for (auto vec : data_vec) {
//        mat[i] = vec;
//        i++;
//    }
//}

//void assigning(double*T, double* mat, int k){
//    for (int i=0; i<16; i++){
//        *T = mat[i+k*16];
//        T++;
//    }
//}



//int main_test(int argc, char* argv[]){
//    string inputfile = "/Users/ryanylj/Desktop/coffee_motion_implementation/eightcircling/circle/invMat.csv";
//    int entries = 16;
//    int timesteps = 360;
//    double mat[entries*timesteps];
//    valuing(inputfile, mat);
    
//    string outputfile = "/Users/ryanylj/Desktop/coffee_motion_implementation/eightcircling/circle/iksolution.csv";
//    ofstream output(outputfile);
    
//    double q_sols[8*6];
//    int num_sols;
    
//    for (int k=0;k<timesteps;k++){
//        double* T = new double[16];
//        assigning(T, mat, k);
//        num_sols = inverse(T, q_sols);
//        for(int i=0;i<6;i++){
//            printf("%1.6f, %1.6f, %1.6f, %1.6f, %1.6f, %1.6f\n",
//                   q_sols[i*6+0], q_sols[i*6+1], q_sols[i*6+2], q_sols[i*6+3], q_sols[i*6+4], q_sols[i*6+5]);
//            output << q_sols[i*6+0] << ", " << q_sols[i*6+1] << ", " << q_sols[i*6+2] << ", " << q_sols[i*6+3] << ", " << q_sols[i*6+4] << ", " << q_sols[i*6+5] << "\n";
//        }
//        printf("\n");
//    }
//    output.close();
    
//    return 0;
//}

int UrKinematics::forward(const double* q, double* T)
{
  double s1 = sin(*q), c1 = cos(*q); q++;
  double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++;
  double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++;
  double s4 = sin(*q), c4 = cos(*q); q234 += *q; q++;
  double s5 = sin(*q), c5 = cos(*q); q++;
  double s6 = sin(*q), c6 = cos(*q);
  double s23 = sin(q23), c23 = cos(q23);
  double s234 = sin(q234), c234 = cos(q234);
  *T = c234*c1*s5 - c5*s1; T++;
  *T = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T++;
  *T = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T++;
  *T = d6*c234*c1*s5 - a3*c23*c1 - a2*c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1; T++;
  *T = c1*c5 + c234*s1*s5; T++;
  *T = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T++;
  *T = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T++;
  *T = d6*(c1*c5 + c234*s1*s5) + d4*c1 - a3*c23*s1 - a2*c2*s1 - d5*s234*s1; T++;
  *T = -s234*s5; T++;
  *T = -c234*s6 - s234*c5*c6; T++;
  *T = s234*c5*s6 - c234*c6; T++;
  *T = d1 + a3*s23 + a2*s2 - d5*(c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4); T++;
  *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
  return 1;
}

int UrKinematics::inverse(const double* T, double* q_sols, double q6_des)
{
    int num_sols = 0;
    double T02 = -*T; T++; double T00 =  *T; T++; double T01 =  *T; T++; double T03 = -*T; T++;
    double T12 = -*T; T++; double T10 =  *T; T++; double T11 =  *T; T++; double T13 = -*T; T++;
    double T22 =  *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 =  *T;

    ////////////////////////////// shoulder rotate joint (q1) //////////////////////////////
    double q1[2];
    {
      double A = d6*T12 - T13;
      double B = d6*T02 - T03;
      double R = A*A + B*B;
      if(fabs(A) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
          div = -SIGN(d4)*SIGN(B);
        else
          div = -d4/B;
        double arcsin = asin(div);
        if(fabs(arcsin) < ZERO_THRESH)
          arcsin = 0.0;
        if(arcsin < 0.0)
          q1[0] = arcsin + 2.0*PI;
        else
          q1[0] = arcsin;
        q1[1] = PI - arcsin;
      }
      else if(fabs(B) < ZERO_THRESH) {
        double div;
        if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
          div = SIGN(d4)*SIGN(A);
        else
          div = d4/A;
        double arccos = acos(div);
        q1[0] = arccos;
        q1[1] = 2.0*PI - arccos;
      }
      else if(d4*d4 > R) {
        return num_sols;
      }
      else {
        double arccos = acos(d4 / sqrt(R)) ;
        double arctan = atan2(-B, A);
        double pos = arccos + arctan;
        double neg = -arccos + arctan;
        if(fabs(pos) < ZERO_THRESH)
          pos = 0.0;
        if(fabs(neg) < ZERO_THRESH)
          neg = 0.0;
        if(pos >= 0.0)
          q1[0] = pos;
        else
          q1[0] = 2.0*PI + pos;
        if(neg >= 0.0)
          q1[1] = neg;
        else
          q1[1] = 2.0*PI + neg;
      }
    }
    ////////////////////////////////////////////////////////////////////////////////

    ////////////////////////////// wrist 2 joint (q5) //////////////////////////////
    double q5[2][2];
    {
      for(int i=0;i<2;i++) {
        double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
        double div;
        if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
          div = SIGN(numer) * SIGN(d6);
        else
          div = numer / d6;
        double arccos = acos(div);
        q5[i][0] = arccos;
        q5[i][1] = 2.0*PI - arccos;
      }
    }
    ////////////////////////////////////////////////////////////////////////////////

    {
      for(int i=0;i<2;i++) {
        for(int j=0;j<2;j++) {
          double c1 = cos(q1[i]), s1 = sin(q1[i]);
          double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
          double q6;
          ////////////////////////////// wrist 3 joint (q6) //////////////////////////////
          if(fabs(s5) < ZERO_THRESH)
            q6 = q6_des;
          else {
            q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
                       SIGN(s5)*(T00*s1 - T10*c1));
            if(fabs(q6) < ZERO_THRESH)
              q6 = 0.0;
            if(q6 < 0.0)
              q6 += 2.0*PI;
          }
          ////////////////////////////////////////////////////////////////////////////////

          double q2[2], q3[2], q4[2];
          ///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////
          double c6 = cos(q6), s6 = sin(q6);
          double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
          double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
          double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
                        T03*c1 + T13*s1;
          double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);

          double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
          if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
            c3 = SIGN(c3);
          else if(fabs(c3) > 1.0) {
            // TODO NO SOLUTION
            continue;
          }
          double arccos = acos(c3);
          q3[0] = arccos;
          q3[1] = 2.0*PI - arccos;
          double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
          double s3 = sin(arccos);
          double A = (a2 + a3*c3), B = a3*s3;
          q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
          q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
          double c23_0 = cos(q2[0]+q3[0]);
          double s23_0 = sin(q2[0]+q3[0]);
          double c23_1 = cos(q2[1]+q3[1]);
          double s23_1 = sin(q2[1]+q3[1]);
          q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
          q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
          ////////////////////////////////////////////////////////////////////////////////
          for(int k=0;k<2;k++) {
            if(fabs(q2[k]) < ZERO_THRESH)
              q2[k] = 0.0;
            else if(q2[k] < 0.0) q2[k] += 2.0*PI;
            if(fabs(q4[k]) < ZERO_THRESH)
              q4[k] = 0.0;
            else if(q4[k] < 0.0) q4[k] += 2.0*PI;
            q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k];
            q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k];
            q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
            num_sols++;
          }

        }
      }
    }
    return num_sols;
  }

